/*
 * motor_control.c
 *
 *  Created on: Nov 4, 2024
 *      Author: ydl43
 */


#include "motor_control.h"

double dtheta = 0;

//=======================================================================================================================================================
//Function:	Obtain the angular velocity of the motor.
//Input:	Motor *motor					A pointer to the motor structure;
//			TIM_HandleTypeDef *htim			An Encoder timer.
void Motor_Speed_Get(Motor *motor, TIM_HandleTypeDef *htim)
{
	motor->Counter_Value = __HAL_TIM_GET_COUNTER(htim);

	if (motor->Dirction == FORWARD_ROTATION)
	{
		if (motor->overflow == NO_OVERFLOW)
		    motor->speed = (2*3.1415927*((double)motor->last_Counter_Value - (double)motor->Counter_Value)/(CIRCLE_LINES*REDUCTION_RATIO*FREQUENCY_MULTIPLICATION))*SAMPLE_FREQUENCY;
		else if (motor->overflow == FORWARD_OVERFLOW)
		{
		    motor->speed = (2*3.1415927*((double)motor->last_Counter_Value - (double)motor->Counter_Value + (double)htim->Instance->ARR)/(CIRCLE_LINES*REDUCTION_RATIO*FREQUENCY_MULTIPLICATION))*SAMPLE_FREQUENCY;
		    motor->overflow = NO_OVERFLOW;
		}
	}
	else if (motor->Dirction == ROLLBACK_ROTATION)
	{
		if (motor->overflow == NO_OVERFLOW)
			motor->speed = (2*3.1415927*((double)motor->last_Counter_Value - (double)motor->Counter_Value)/(CIRCLE_LINES*REDUCTION_RATIO*FREQUENCY_MULTIPLICATION))*SAMPLE_FREQUENCY;
		else if (motor->overflow == ROLLBACK_OVERFLOW)
		{
			motor->speed = (2*3.1415927*((double)motor->last_Counter_Value - (double)motor->Counter_Value - (double)htim->Instance->ARR)/(CIRCLE_LINES*REDUCTION_RATIO*FREQUENCY_MULTIPLICATION))*SAMPLE_FREQUENCY;
			motor->overflow = NO_OVERFLOW;
		}
	}
	else if (motor->Dirction == STOP_ROTATION)
	{
		motor->speed = 0;
		motor->overflow = NO_OVERFLOW;
	}
	motor->last_Counter_Value = motor->Counter_Value;
}
//=======================================================================================================================================================

//=======================================================================================================================================================
//Function:	Obtain the angular displacement of the motor.
//Input:	Motor *motor					A pointer to the motor structure;
//			TIM_HandleTypeDef *htim			An Encoder timer.
void Motor_Angle_Get(Motor *motor, TIM_HandleTypeDef *htim)
{
	Motor_Speed_Get(motor, htim);
	dtheta = (motor->speed / (double)SAMPLE_FREQUENCY);
	motor->angle += dtheta;
}
//=======================================================================================================================================================

//=======================================================================================================================================================
//Function:	Change the direction of motor rotation
//Input:	Motor *motor					A pointer to the motor structure;
//			uint8_t dir						The direction of turning.
void Motor_Direction_Control(Motor *motor, uint8_t dir)
{
	if (dir == STOP_ROTATION)
	{
		HAL_GPIO_WritePin(motor->IN1_Port, motor->IN1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(motor->IN2_Port, motor->IN2_Pin, GPIO_PIN_RESET);
	}
	else if (dir == FORWARD_ROTATION)
	{
		HAL_GPIO_WritePin(motor->IN1_Port, motor->IN1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(motor->IN2_Port, motor->IN2_Pin, GPIO_PIN_RESET);
	}
	else if (dir == ROLLBACK_ROTATION)
	{
		HAL_GPIO_WritePin(motor->IN1_Port, motor->IN1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(motor->IN2_Port, motor->IN2_Pin, GPIO_PIN_SET);
	}
	motor->Dirction = dir;
}
//=======================================================================================================================================================

//=======================================================================================================================================================
//Function:	Motor angular velocity PID control.
//Input:	Motor *motor					A pointer to the motor structure;
//			PID *pid						An angular velocity PID control structure;
//			TIM_HandleTypeDef *htim			A Timer that generates PWM waves;
//			uint32_t channel				A Timer Channel that generates PWM waves;
//			double target_Speed				The Target speed.
void Motor_Speed_Control(Motor *motor, PID *pid, TIM_HandleTypeDef *htim, uint32_t channel, double target_Speed)
{
	motor->target_Speed = target_Speed;

	pid->Error = target_Speed - motor->speed;

	motor->target_PWM += pid->P * (pid->Error - pid->Last_Error) + pid->I * pid->Error + pid->D * (pid->Error - 2 * pid->Last_Error + pid->Last_2_Error);

	double Abs_target_PWM = (motor->target_PWM > 0) ? motor->target_PWM : -motor->target_PWM;
	if (motor->target_PWM > 0) motor->Dirction = FORWARD_ROTATION;
	else if (motor->target_PWM < 0) motor->Dirction = ROLLBACK_ROTATION;
	else motor->Dirction = STOP_ROTATION;
	Motor_Direction_Control(motor, motor->Dirction);

	if (Abs_target_PWM > 100) Abs_target_PWM = 100;

	__HAL_TIM_SET_COMPARE(htim, channel, (uint32_t)((Abs_target_PWM/100) * htim->Init.Period));

	pid->Last_2_Error = pid->Last_Error;
	pid->Last_Error = pid->Error;
}
//=======================================================================================================================================================

//=======================================================================================================================================================
//Function:	Cascade PID control of the angular displacement of the motor.
//Input:	Motor *motor					A pointer to the motor structure;
//			PID *pid_Angle					An angular displacement PID control structure;
//			PID *pid_Speed					An angular velocity PID control structure;
//			TIM_HandleTypeDef *htim			A Timer that generates PWM waves;
//			uint32_t channel				A Timer Channel that generates PWM waves;
//			double target_Angle				The Target speed.
void Motor_Angle_Control(Motor *motor, PID *pid_Angle, PID *pid_Speed, TIM_HandleTypeDef *htim, uint32_t channel, double target_Angle)
{
	pid_Angle->Error = target_Angle - motor->angle;

	motor->target_Speed += pid_Angle->P * (pid_Angle->Error - pid_Angle->Last_Error) + pid_Angle->I * pid_Angle->Error + pid_Angle->D * (pid_Angle->Error - 2 * pid_Angle->Last_Error + pid_Angle->Last_2_Error);
    if (motor->target_Speed > MAX_SPEED) motor->target_Speed = MAX_SPEED;
    else if (motor->target_Speed < -MAX_SPEED) motor->target_Speed = -MAX_SPEED;

	Motor_Speed_Control(motor, pid_Speed, htim, channel, motor->target_Speed);

	pid_Angle->Last_2_Error = pid_Angle->Last_Error;
	pid_Angle->Last_Error = pid_Angle->Error;
}
//=======================================================================================================================================================
